#include <stdio.h>
#include <string.h>
#include <ctype.h>
#include <vector>


class Vertex
{
	public:
        
	float x; ///<wspolrzedna x wierzcholka
        float y; ///<wspolrzedna y wierzcholka
        float z; ///<wspolrzedna z wierzcholka

        /**
         * Konstruktor glowny - inicjuje 3 zadanymi wartosciami
         */
	Vertex(float x, float y, float z);

        /**
         * Konstruktor domyslny - inicjuje wierzcholek (0,0,0)
         */
        Vertex(void);


};

Vertex::Vertex(float x, float y, float z):
	x(x),
	y(y),
	z(z)
{};

Vertex::Vertex(void): x(0), y(0), z(0)
{};


class Triangle
{
	public:
        /// normalna rzeczywista
	Vertex normal;
        /// normalna zerowa
        Vertex normal0;
        /// wierzcholki rzeczywiste
	Vertex vertex[3];
        /// wierzcholki zerowe
        Vertex vertex0[3];

        /**
         * @param n[]   wspolrzedne normalnej (x,y,z)
         * @param v0[], v1[], v2[]  wspolrzedne kolejnych wierzcholkow
         */
	Triangle(float n[], float v0[], float v1[], float v2[]);
};

Triangle::Triangle(float n[], float x[], float y[], float z[]):
	normal(n[0],n[1],n[2]), normal0(n[0],n[1],n[2])
{
	vertex[0]=Vertex(x[0],y[0],z[0]);
	vertex[1]=Vertex(x[1],y[1],z[1]);
	vertex[2]=Vertex(x[2],y[2],z[2]);

        vertex0[0]=Vertex(x[0],y[0],z[0]);
	vertex0[1]=Vertex(x[1],y[1],z[1]);
	vertex0[2]=Vertex(x[2],y[2],z[2]);
}


//-----------------------------------------------------------------------------
std::vector< Triangle > triangles;
std::vector< Vertex > vertices;
std::vector< Vertex > normals;
std::vector< int > indices;

//-----------------------------------------------------------------------------

int verticesCount=0;

int addToVertices(float x, float y, float z)
{
	unsigned int i=0;
	
	for (i=0; i<vertices.size(); i++)
	{
		if (vertices[i].x==x && vertices[i].y==y && vertices[i].z==z)
			return i;
	}
	
	vertices.push_back(Vertex(x,y,z));	
	verticesCount++;
	
	return i;	
}


int readTrianglesFromStl(char * filepath, float tx, float ty, float tz)
{
    char * solid = "solid ascii";
    char * endsolid = "endsolid";

    char * facet = "facet normal";
    char * endfacet = "endfacet";

    char * loop = "outer loop";
    char * endloop = "endloop";

    char * vertex_str = "vertex";

    char buf[80];

    FILE * f = fopen(filepath, "r");

    if (!f)
    {
        printf("Blad: nie istnieje plik %s\n",filepath);
        return -1;
    }

    //float n[3], x[3], y[3], z[3];
	float x,y,z;

    // count = 0;

    int vertexCount=0;
    int trianglesCount=0;
	int index;
	
    // parsowanie pliku stl line-by-line
    while (fgets(buf, 80, f))
    {
        // wskaznik roboczy
        char *bufWork = buf;

        // usuniecie poprzedzajacych spacji
        while (*(bufWork++) == ' ');
        bufWork--;

        // sprawdzanie "sygnatury" linii
        if (!strncmp(bufWork, vertex_str, strlen(vertex_str)))
        {
			// sprawdzamy czy mamy max 3 wierzcholki
            if (vertexCount>2)
                printf("Blad: Vertex Count!\n"); fflush(stdout);

            // usuniecie znakow niebedacych cyframi
            while (!isdigit(*(bufWork++)));
            bufWork -= 2;

        //    sscanf(bufWork, "%e %e %e", &x[vertexCount], &y[vertexCount], &z[vertexCount]);
        //    x[vertexCount]+=tx; y[vertexCount]+=ty; z[vertexCount]+=tz;
		
			sscanf(bufWork, "%e %e %e", &x, &y, &z);
			index = addToVertices(x,y,z);
			indices.push_back(index);
			
            //printf("vertex: %e %e %e\n", x[vertexCount], y[vertexCount], z[vertexCount]);

            vertexCount++;

        } else if (!strncmp(bufWork, solid, strlen(solid)))
            ;//puts("START");
        else if (!strncmp(bufWork, endsolid, strlen(endsolid)))
            ;//puts("STOP");
        else if (!strncmp(bufWork, facet, strlen(facet)))
        {
            vertexCount=0;
            // usuniecie znakow niebedacych cyframi
            while (!isdigit(*(bufWork++)));
            bufWork -= 2;

// todo           sscanf(bufWork, "%e %e %e", &n[0], &n[1], &n[2]);
        //    printf("normal: %e %e %e\n", n[0], n[1], n[2]);
        } else if (!strncmp(bufWork, endfacet, strlen(endfacet)))
        {
            //puts("EF");
// todo            triangles.push_back(Triangle(n,x,y,z));
			trianglesCount++;
        }
        else if (!strncmp(bufWork, loop, strlen(loop)))
            ;//puts("LOOP");
        else if (!strncmp(bufWork, endloop, strlen(endloop)))
            ;//puts("EndLOOP");
        else
            printf("%s: %s\n",filepath,bufWork);

    }

    fclose(f);
    printf("%s: %i/%i\n",filepath,verticesCount,trianglesCount);
    return trianglesCount;
}


int readPly(char * filepath, float tx, float ty, float tz)
{
	

    char * element_vertex = "element vertex";
    char * element_face = "element face";
	char * end_header = "end_header";

    char buf[80];

    FILE * f = fopen(filepath, "r");

    if (!f)
    {
        printf("Blad: nie istnieje plik %s\n",filepath);
        return -1;
    }

    int vertexCount=0;
	int faceCount=0;    	
	
    // parsowanie pliku stl line-by-line
    while (fgets(buf, 80, f))
    {
        // wskaznik roboczy
        char *bufWork = buf;

        // usuniecie poprzedzajacych spacji
        while (*(bufWork++) == ' ');
        bufWork--;

		if (!strncmp(bufWork, element_vertex, strlen(element_vertex)))
		{
			// usuniecie znakow niebedacych cyframi
            while (!isdigit(*(bufWork++)));
            bufWork -= 1;
			
			sscanf(bufWork, "%i", &vertexCount);			
		}
		else if (!strncmp(bufWork, element_face, strlen(element_face)))
		{
			// usuniecie znakow niebedacych cyframi
            while (!isdigit(*(bufWork++)));
            bufWork -= 1;
			
			sscanf(bufWork, "%i", &faceCount);
		}
		else if (!strncmp(bufWork, end_header, strlen(end_header)))
			break;
	}
		
	int i;	
		    

	float verticesArray[vertexCount*3];
	float normalsArray[vertexCount*3];
	
	float * vertexPtr = &verticesArray[0];
	float * normalPtr = &normalsArray[0];
	
	// vertices
	for (i=0; i<vertexCount; i++)
	{
		fgets(buf, 80, f);
		sscanf(buf, "%f %f %f %f %f %f", vertexPtr, vertexPtr+1, vertexPtr+2, normalPtr, normalPtr+1, normalPtr+2);
		vertexPtr += 3;	
		normalPtr += 3;
	}


	int dump;	
	int indexes[faceCount*3];	
	int * indexPtr = &indexes[0];
	
	// indices
	for (i=0; i<faceCount; i++)
	{
		fgets(buf, 80, f);
		sscanf(buf,"%i %i %i %i", &dump, indexPtr, indexPtr+1, indexPtr+2); 
		indexPtr+=3;
	}


    fclose(f);
    printf("%s: %i/%i\n",filepath,vertexCount,faceCount);
    return faceCount;
}


int main(void)
{
	
	return readPly("../RobotCollisionDetection/kawasaki/czlon_1.ply",0,0,0);
}
